#include <rtthread.h>
#include <rtdevice.h>

#include "ov2640_regs.h"

struct regval
{
	uint8_t reg;
	uint8_t value;
};

#define ENDMARKER { 0x00, 0x00 }

#define CIF_WIDTH       (400)
#define CIF_HEIGHT      (296)

#define SVGA_WIDTH      (800)
#define SVGA_HEIGHT     (600)

#define QVGA_WIDTH      (320)
#define QVGA_HEIGHT     (240)

#define UXGA_WIDTH      (1600)
#define UXGA_HEIGHT     (1200)

static const struct regval ov2640_init_regs[] = {
	{ BANK_SEL, BANK_SEL_DSP },
	{ 0x2c,   0xff },
	{ 0x2e,   0xdf },

	{ BANK_SEL, BANK_SEL_SENS },
	{ 0x3c,   0x32 },
	{ CLKRC,  CLKRC_DIV_SET(1) },
	{ COM2,   COM2_OCAP_Nx_SET(3) },
	{ REG04,  REG04_VREF_EN | REG04_VFLIP_IMG | REG04_HREF_EN },
	{ COM8,   COM8_DEF | COM8_BNDF_EN | COM8_AGC_EN | COM8_AEC_EN },
	{ COM9,   COM9_AGC_GAIN_8x | 0x08},
	{ 0x2c,   0x0c },
	{ 0x33,   0x78 },
	{ 0x3a,   0x33 },
	{ 0x3b,   0xfb },
	{ 0x3e,   0x00 },
	{ 0x43,   0x11 },
	{ 0x16,   0x10 },
	{ 0x39,   0x02 },
	{ 0x35,   0x88 },
	{ 0x22,   0x0a },
	{ 0x37,   0x40 },
	{ 0x23,   0x00 },
	{ ARCOM2, 0xa0 },
	{ 0x06,   0x02 },
	{ 0x06,   0x88 },
	{ 0x07,   0xc0 },
	{ 0x0d,   0xb7 },
	{ 0x0e,   0x01 },
	{ 0x4c,   0x00 },
	{ 0x4a,   0x81 },
	{ 0x21,   0x99 },
	{ AEW,    0x40 },
	{ AEB,    0x38 },
	{ VV,     VV_HIGH_TH_SET(0x08) | VV_LOW_TH_SET(0x02) },
	{ 0x5c,   0x00 },
	{ 0x63,   0x00 },
	{ FLL,    0x22 },
	{ COM3,   0x38 | COM3_BAND_AUTO },
	{ REG5D,  0x55 },
	{ REG5E,  0x7d },
	{ REG5F,  0x7d },
	{ REG60,  0x55 },
	{ HISTO_LOW,   0x70 },
	{ HISTO_HIGH,  0x80 },
	{ 0x7c,   0x05 },
	{ 0x20,   0x80 },
	{ 0x28,   0x30 },
	{ 0x6c,   0x00 },
	{ 0x6d,   0x80 },
	{ 0x6e,   0x00 },
	{ 0x70,   0x02 },
	{ 0x71,   0x94 },
	{ 0x73,   0xc1 },
	{ 0x3d,   0x34 },
	{ COM7,   COM7_RES_UXGA | COM7_ZOOM_EN },
	{ REG5A,  BD50_MAX_AEC_STEP_SET(6)
		   | BD60_MAX_AEC_STEP_SET(8) },		/* 0x57 */
	{ COM25,  COM25_50HZ_BANDING_AEC_MSBS_SET(0x0bb)
		   | COM25_60HZ_BANDING_AEC_MSBS_SET(0x09c) },	/* 0x00 */
	{ BD50,   BD50_50HZ_BANDING_AEC_LSBS_SET(0x0bb) },	/* 0xbb */
	{ BD60,   BD60_60HZ_BANDING_AEC_LSBS_SET(0x09c) },	/* 0x9c */
	{ BANK_SEL,  BANK_SEL_DSP },
	{ 0xe5,   0x7f },
	{ MC_BIST,  MC_BIST_RESET | MC_BIST_BOOT_ROM_SEL },
	{ 0x41,   0x24 },
	{ RESET,  RESET_JPEG | RESET_DVP },
	{ 0x76,   0xff },
	{ 0x33,   0xa0 },
	{ 0x42,   0x20 },
	{ 0x43,   0x18 },
	{ 0x4c,   0x00 },
	{ CTRL3,  CTRL3_BPC_EN | CTRL3_WPC_EN | 0x10 },
	{ 0x88,   0x3f },
	{ 0xd7,   0x03 },
	{ 0xd9,   0x10 },
	{ R_DVP_SP,  R_DVP_SP_AUTO_MODE | 0x2 },
	{ 0xc8,   0x08 },
	{ 0xc9,   0x80 },
	{ BPADDR, 0x00 },
	{ BPDATA, 0x00 },
	{ BPADDR, 0x03 },
	{ BPDATA, 0x48 },
	{ BPDATA, 0x48 },
	{ BPADDR, 0x08 },
	{ BPDATA, 0x20 },
	{ BPDATA, 0x10 },
	{ BPDATA, 0x0e },
	{ 0x90,   0x00 },
	{ 0x91,   0x0e },
	{ 0x91,   0x1a },
	{ 0x91,   0x31 },
	{ 0x91,   0x5a },
	{ 0x91,   0x69 },
	{ 0x91,   0x75 },
	{ 0x91,   0x7e },
	{ 0x91,   0x88 },
	{ 0x91,   0x8f },
	{ 0x91,   0x96 },
	{ 0x91,   0xa3 },
	{ 0x91,   0xaf },
	{ 0x91,   0xc4 },
	{ 0x91,   0xd7 },
	{ 0x91,   0xe8 },
	{ 0x91,   0x20 },
	{ 0x92,   0x00 },
	{ 0x93,   0x06 },
	{ 0x93,   0xe3 },
	{ 0x93,   0x03 },
	{ 0x93,   0x03 },
	{ 0x93,   0x00 },
	{ 0x93,   0x02 },
	{ 0x93,   0x00 },
	{ 0x93,   0x00 },
	{ 0x93,   0x00 },
	{ 0x93,   0x00 },
	{ 0x93,   0x00 },
	{ 0x93,   0x00 },
	{ 0x93,   0x00 },
	{ 0x96,   0x00 },
	{ 0x97,   0x08 },
	{ 0x97,   0x19 },
	{ 0x97,   0x02 },
	{ 0x97,   0x0c },
	{ 0x97,   0x24 },
	{ 0x97,   0x30 },
	{ 0x97,   0x28 },
	{ 0x97,   0x26 },
	{ 0x97,   0x02 },
	{ 0x97,   0x98 },
	{ 0x97,   0x80 },
	{ 0x97,   0x00 },
	{ 0x97,   0x00 },
	{ 0xa4,   0x00 },
	{ 0xa8,   0x00 },
	{ 0xc5,   0x11 },
	{ 0xc6,   0x51 },
	{ 0xbf,   0x80 },
	{ 0xc7,   0x10 },	/* simple AWB */
	{ 0xb6,   0x66 },
	{ 0xb8,   0xA5 },
	{ 0xb7,   0x64 },
	{ 0xb9,   0x7C },
	{ 0xb3,   0xaf },
	{ 0xb4,   0x97 },
	{ 0xb5,   0xFF },
	{ 0xb0,   0xC5 },
	{ 0xb1,   0x94 },
	{ 0xb2,   0x0f },
	{ 0xc4,   0x5c },
	{ 0xa6,   0x00 },
	{ 0xa7,   0x20 },
	{ 0xa7,   0xd8 },
	{ 0xa7,   0x1b },
	{ 0xa7,   0x31 },
	{ 0xa7,   0x00 },
	{ 0xa7,   0x18 },
	{ 0xa7,   0x20 },
	{ 0xa7,   0xd8 },
	{ 0xa7,   0x19 },
	{ 0xa7,   0x31 },
	{ 0xa7,   0x00 },
	{ 0xa7,   0x18 },
	{ 0xa7,   0x20 },
	{ 0xa7,   0xd8 },
	{ 0xa7,   0x19 },
	{ 0xa7,   0x31 },
	{ 0xa7,   0x00 },
	{ 0xa7,   0x18 },
	{ 0x7f,   0x00 },
	{ 0xe5,   0x1f },
	{ 0xe1,   0x77 },
	{ 0xdd,   0x7f },
	{ CTRL0,  CTRL0_YUV422 | CTRL0_YUV_EN | CTRL0_RGB_EN },

	ENDMARKER,
};

/*
 * Register settings for window size
 * The preamble, setup the internal DSP to input an UXGA (1600x1200) image.
 * Then the different zooming configurations will setup the output image size.
 */
static const struct regval ov2640_size_change_preamble_regs[] = {
	{ BANK_SEL, BANK_SEL_DSP },
	{ RESET, RESET_DVP },
	{ SIZEL, SIZEL_HSIZE8_11_SET(UXGA_WIDTH) |
		 SIZEL_HSIZE8_SET(UXGA_WIDTH) |
		 SIZEL_VSIZE8_SET(UXGA_HEIGHT) },
	{ HSIZE8, HSIZE8_SET(UXGA_WIDTH) },
	{ VSIZE8, VSIZE8_SET(UXGA_HEIGHT) },
	{ CTRL2, CTRL2_DCW_EN | CTRL2_SDE_EN |
		 CTRL2_UV_AVG_EN | CTRL2_CMX_EN | CTRL2_UV_ADJ_EN },
	{ HSIZE, HSIZE_SET(UXGA_WIDTH) },
	{ VSIZE, VSIZE_SET(UXGA_HEIGHT) },
	{ XOFFL, XOFFL_SET(0) },
	{ YOFFL, YOFFL_SET(0) },
	{ VHYX, VHYX_HSIZE_SET(UXGA_WIDTH) | VHYX_VSIZE_SET(UXGA_HEIGHT) |
		VHYX_XOFF_SET(0) | VHYX_YOFF_SET(0)},
	{ TEST, TEST_HSIZE_SET(UXGA_WIDTH) },

	ENDMARKER,
};

#define PER_SIZE_REG_SEQ(x, y, v_div, h_div, pclk_div)	\
	{ CTRLI, CTRLI_LP_DP | CTRLI_V_DIV_SET(v_div) |	\
		 CTRLI_H_DIV_SET(h_div)},		\
	{ ZMOW, ZMOW_OUTW_SET(x) },			\
	{ ZMOH, ZMOH_OUTH_SET(y) },			\
	{ ZMHH, ZMHH_OUTW_SET(x) | ZMHH_OUTH_SET(y) },	\
	{ R_DVP_SP, pclk_div },				\
	{ RESET, 0x00}

static const struct regval ov2640_qvga_regs[] = {
	PER_SIZE_REG_SEQ(QVGA_WIDTH, QVGA_HEIGHT, 2, 2, 4),

	ENDMARKER,
};

/*
 * Register settings for pixel formats
 */
static const struct regval ov2640_format_change_preamble_regs[] = {
	{ BANK_SEL, BANK_SEL_DSP },
	{ R_BYPASS, R_BYPASS_USE_DSP },

	ENDMARKER,
};

static const struct regval ov2640_rgb565_be_regs[] = {
	{ IMAGE_MODE, IMAGE_MODE_RGB565 },
	{ 0xd7, 0x03 },
	{ RESET,  0x00 },
	{ R_BYPASS, R_BYPASS_USE_DSP },
	ENDMARKER,
};

static const struct regval ov2640_rgb565_le_regs[] = {
	{ IMAGE_MODE, IMAGE_MODE_LBYTE_FIRST | IMAGE_MODE_RGB565 },
	{ 0xd7, 0x03 },
	{ RESET,  0x00 },
	{ R_BYPASS, R_BYPASS_USE_DSP },

	ENDMARKER,
};

static int reg_read(rt_video_t *vid, uint16_t reg, uint8_t *d)
{
    int ret = 0;

    *d = 0;
    ret |= rt_camif_reg_read(vid->ci, vid->ci->slv_addr, reg, d);

    return ret;
}

static int reg_write(rt_video_t *vid, uint16_t reg, uint8_t d)
{
    return rt_camif_reg_write(vid->ci, vid->ci->slv_addr, reg, &d);
}

static int reg_read_e1(rt_video_t *vid, uint16_t reg, uint8_t *d)
{
    reg_read(vid, reg, d);

    return ((*d != 0) && (*d != 0xff));
}

static int ov2640_write_array(rt_video_t *vid, const struct regval *vals)
{
	int ret = 0;

	while ((vals->reg != 0x00) || (vals->value != 0x00)) 
    {
        ret = reg_write(vid, vals->reg, vals->value);
		if (ret != 0)
        {
            break;
        }

		vals++;
	}

	return ret;
}

static int ov2640_mask_set(rt_video_t *vid, uint16_t reg, uint8_t mask, uint8_t set)
{
	uint8_t val;

	if (reg_read(vid, reg, &val) != 0)
		return -1;

	val &= ~mask;
	val |= set & mask;

	return reg_write(vid, reg, val);
}

static int _set_img_format(rt_video_t *vid, struct video_format *fmt)
{
    int ret = 0;
    struct video_pix_format *pf = &fmt->fmt.pix;

    switch (pf->pixelformat)
    {
    case VIDEO_PIX_FMT_RGB565:
    {
        
    }break;
    default:
    {
        ret = -EINVAL;
    }break; 
    }

    ret = ov2640_write_array(vid, ov2640_format_change_preamble_regs);
    ret = ov2640_write_array(vid, ov2640_rgb565_be_regs);

    return ret;
}

static int _set_img_size(rt_video_t *vid, struct video_format *fmt)
{
    int ret = 0;
    uint16_t w, h;

    w = fmt->fmt.pix.width;
    h = fmt->fmt.pix.height;

    if ((w & 0x03) || (h & 0x03) || (w > UXGA_WIDTH) || (h > UXGA_HEIGHT)) 
    { // w/h must be divisble by 4
        return -EINVAL;
    }

    ret = ov2640_write_array(vid, ov2640_size_change_preamble_regs);
    ret = ov2640_write_array(vid, ov2640_qvga_regs);

    return ret;
}

static int reg_read16(rt_camif_t *ci, uint16_t reg, uint16_t *d)
{
    uint8_t tmp = 0;
    int ret = 0;

    *d = 0;
    ret |= rt_camif_reg_read(ci, ci->slv_addr, reg, &tmp);
    *d = tmp << 8;
    ret |= rt_camif_reg_read(ci, ci->slv_addr, reg + 1, &tmp);
    *d |= tmp;    

    return ret;
}

static int _readid(rt_camif_t *ci, uint32_t *id)
{
    uint16_t val;
    int ret = -1;

    reg_read16(ci, 0x0A, &val);

    if ((val == 0x2640) || (val == 0x2641) || (val == 0x2642))
    {
        ret = 0;
        if (id)
            *id = val;
    }

    return ret;
}

static int ov_init(rt_video_t *vid)
{
    int ret = 0;

    rt_camif_set_xclk(vid->ci, 24000000);

    ret |= reg_write(vid, BANK_SEL, BANK_SEL_SENSOR);
    ret |= reg_write(vid, COM7, COM7_SRST);

    rt_thread_mdelay(20);

    ret = ov2640_write_array(vid, ov2640_init_regs);

    return ret;
}

static void ov_deinit(rt_video_t *vid)
{

}

static int ov_set_format(rt_video_t *vid, struct video_format *fmt)
{
    int ret;

    ret = _set_img_size(vid, fmt);
    if (ret != 0)
        goto _out;

    ret = _set_img_format(vid, fmt);
    ret = ov2640_mask_set(vid, CTRL0, CTRL0_VFIRST, 0);

_out:
    return ret;
}

static int ov_get_format(rt_video_t *vid, struct video_format *fmt)
{
    int ret = -EINVAL;
    uint16_t w, h;
#if 0
    if (!reg_read_e1(vid, 0x3808, &w)) //width
        goto _out;
    if (!reg_read_e1(vid, 0x380A, &h)) //height
        goto _out;
#endif
    fmt->fmt.pix.width = 320;
    fmt->fmt.pix.height = 240;
    fmt->fmt.pix.bytesperline = fmt->fmt.pix.width * 2;
    ret = 0;

_out:
    return ret;
}

static int ov_stream_enable(rt_video_t *vid, int en)
{
    return 0;
}

static const struct video_ops _ops =
{
    .init = ov_init,
    .deinit = ov_deinit,
    .set_format = ov_set_format,
    .get_format = ov_get_format,
    .stream_enable = ov_stream_enable,
};

int ov2640_probe(rt_camif_t *ci, const struct video_ops **ops, uint32_t *id)
{
    int ret;

    ci->slv_addr = 0x60;

    ret = _readid(ci, id);
    if (ret == 0 && ops)
    {
        *ops = &_ops;
    }

    return ret;
}
